#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "defns5.h"

#define ROTATE(a,i,j,k,l) { g       = a[i][j] ;         \
                            h       = a[k][l] ;         \
                            a[i][j] = g-s*(h+g*tau) ;   \
                            a[k][l] = h+s*(g-h*tau) ;   \
                          }

double  *dvector(int)           ; 
double **matrix(int,int)        ;
void     nerror(char *)         ;
void     jacobi(double **,double *,double **,int,int *) ; 
void     free_dvector(double *) ; 
void     eigsrt(double *,double **,int) ; 


void eig_jacobi(
double **a,
double  *d,
double **v,
int      n,
int     *nrot)
{

   int     j,iq,ip,i;
   double  tresh,theta,tau,t,sm,s,h,g,c,*b,*z ; 
   
   
   b = dvector(n) ; 
   z = dvector(n) ; 
   
   /* initialize to identity matrix */
   for(ip=1 ; ip <=n ; ip++) {
     for(iq=1 ; iq <= n ; iq++) 
        v[ip][iq] = 0.0 ; 
     v[ip][ip] = 1.0 ; 
   }

   /* initialize b and d to the diagonal of a */
   for(ip=1 ; ip <=n ; ip++) {
      b[ip] = d[ip] = a[ip][ip] ; 
      z[ip] = 0.0 ; 
   }

   *nrot = 0 ; 
   for(i=1 ; i <= 70 ; i++) {
     sm = 0.0 ; 
     for(ip=1 ; ip <= n-1 ; ip++) {
       for(iq=ip+1 ; iq <= n ; iq++) 
          sm += fabs(a[ip][iq]) ; 
     }
     if (sm < 1.0e-5) {
       free_dvector(z) ; 
       free_dvector(b) ; 
       return ; 
     }
     if (i < 4)                /* on the first 3 sweeps */
       tresh = 0.2*sm/(n*n) ; 
     else 
       tresh = 0.0 ;           /* thereafter */
     for(ip = 1 ; ip <=n-1 ; ip++) {
       for(iq=ip+1 ; iq <= n ; iq++) {
          g=100.0*fabs(a[ip][iq]) ; 
          if(i > 4 && (double)(fabs(d[ip])+g) == (double) fabs(d[ip]) 
             && (double)(fabs(d[iq])+g) == (double)fabs(d[iq])) 
               a[ip][iq] = 0.0 ; 
          else if (fabs(a[ip][iq]) > tresh) {
               h = d[iq]-d[ip] ; 
               if ( (double) (fabs(h)+g) == (double) fabs(h) ) 
                  t = (a[ip][iq])/h ; 
               else {
                  theta = 0.5*h/(a[ip][iq]) ; 
                  t = 1.0/(fabs(theta)+sqrt(1.0+theta*theta)) ; 
                  if (theta < 0.0) t = -t ; 
               }
          }
          c = 1.0/sqrt(1+t*t) ; 
          s=t*c ; 
          tau=s/(1.0+c) ; 
          h=t*a[ip][iq] ; 
          z[ip] -= h ; 
          z[iq] += h ; 
          d[ip] -= h ; 
          d[iq] += h ; 
          a[ip][iq] = 0.0 ; 
          for(j=1 ; j <= ip-1 ; j++) {
             ROTATE(a,j,ip,j,iq)   ;
          }
          for(j=ip+1 ; j <= iq-1 ; j++) {
             ROTATE(a,ip,j,j,iq)  ;
          }
          for(j=iq+1 ; j <= n ; j++) {
             ROTATE(a,ip,j,iq,j)  ;
          }
          for(j=1 ; j <= n ; j++) {
             ROTATE(v,j,ip,j,iq) ; 
          }
          ++(*nrot) ; 
       }
     }
   }
   for(ip=1 ; ip <= n ; ip++) {
     b[ip] += z[ip] ; 
     d[ip] = b[ip] ; 
     z[ip] = 0.0 ; 
   }
   printf("Too many iterations in routine JACOBI") ; 
}

/*
 *    Sorts the eigenvalues and eigenvectors.
 */
void eigsrt(
double *d,
double **v,
int n)
{
   int k, j,i ; 
   double p ; 
   
   for(i=1 ; i < n ; i++) { 
     p=d[k=i] ; 
     for(j=i+1 ; j <= n ; j++) 
       if(d[j] < p) p=d[k=j] ; 
     if(k != i) {
       d[k] = d[i] ; 
       d[i] = p ; 
       for(j=1 ; j <= n ; j++) {
         p=v[j][i] ; 
         v[j][i] = v[j][k] ; 
         v[j][k] = p ; 
       }
     }
   }
}

double *dvector(
int n)
{
  double *t ;
 
  t = (double *) calloc(n+1,sizeof(double)) ;
  if (t == NULL) {
    printf("Error: Memory alloc error (dvector)\n") ; 
    abort() ; 
  }
  return(t) ;
}

void free_dvector(
double *t)
{
  free(t) ;
}

double **matrix(
int n,
int m)
{
   int i ;
   double **t ;
 
   t = (double **) calloc(n+1,sizeof(double *)) ;
   if (t == NULL) {
      printf("Error: Memory alloc error (matrix)") ;
      abort() ; 
   }    
   for(i=1 ; i <= n ; i++) {
      t[i] = (double *) calloc(m+1,sizeof(double)) ;
      if (t == NULL) { 
        printf("Error: Memory alloc error (matrix)") ;  
        abort() ; 
      }    
   }
   return(t) ;
}

void free_matrix(
double **t,
int      n)
{
   int i ; 
   
   for(i=1 ; i <= n ; i++) 
     free(t[i]) ;  
   free(t) ; 
}
